/********** Local Includes ****************************************************/

#include "approxDecomp.h"
#include "objectdetector.h"
#include "common.h"
#include "imagesource.h"
#include "robot_orientation.h"

/********** Macros and Constants **********************************************/

#define BLUE cvScalar(255, 0, 0)
#define GREEN cvScalar(0, 255, 0)

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Function Prototypes *****************************************/

/**
 * Matlab-style all() function.
 *
 * Returns true if all of the values in the matrix are non-zero, or false
 * otherwise.
 *
 * The given matrix should be type CV_32FC1.
 */
bool all(const Mat &);

/********** Public Methods ****************************************************/

ApproxDecomp::ApproxDecomp(ImageSource *source, Mat open) :
m_source(source),
m_open(open)
{
	if (!m_source) {
		ERROR("Image source cannot be null");
	}
	
	
}


Mat ApproxDecomp::cellDecomp() {
    
    
    
    Size gridSize = m_open.size();
    int rows =gridSize.height/blockSize();
    int cols = gridSize.width/blockSize();
     cout << "Grid has " << rows<< " rows and "<< cols<< "cols."<< endl;
    
    Mat grid(Size(cols, rows), CV_32FC1);
    grid = 1;
    
    Mat temp;
    cvtColor(m_open, temp, CV_GRAY2BGR);

    //in m_open 1 means pixel is not changed(no obstacle)
    //in grid 0 means blocked
    int currow= 0;
    int curcol = 0;
    
    for(int i = 0; i + blockSize() - 1 < gridSize.height; i += blockSize())//iterate through blocks
	{
		curcol = 0;
		
		for(int j = 0; j + blockSize() - 1 < gridSize.width; j += blockSize())   
		{
			// Copy the one block into a new matrix
			Mat region = m_open(Range(i, i + blockSize() - 1),
				Range(j, j + blockSize() - 1));
            
			if (all(region)) {
				grid.at<float>(currow, curcol) = 1;
			} else {
				grid.at<float>(currow, curcol) = 0;
			}
            
            curcol++;
        }
        
        currow++;
    }
	
    for(int n= 0; n< rows; n++){
        for(int p = 0; p < cols; p++){
            if( grid.at<float>( n,p) == 0){//blocked cell
				//draw over it
				int startRow= (n)*blockSize();
				int startCol =(p)*blockSize() ;
				//Point start(startRow,startCol);
				//Point end(startRow+blockSize(), startCol+blockSize());
				Point start(startCol, startRow);
				Point end(startCol + blockSize(), startRow + blockSize());
				rectangle(temp, start,end,BLUE, CV_FILLED);
            } else {
                //draw over it
                int startRow= (n)*blockSize();
                int startCol =(p)*blockSize();              
				Point start(startCol, startRow);
				Point end(startCol + blockSize(), startRow + blockSize());
                rectangle(temp, start,end,GREEN);
            }
            
        }
        
    }
    
    imshow("blocked", temp);
    cvMoveWindow("blocked", temp.size().width, 250);

	return grid;
}

int ApproxDecomp::blockSize() const {
	return 20;
}


/********** Private Methods ***************************************************/



/********** Local Functions ***************************************************/

bool all(const Mat &matrix) {
	for (int row = 0; row < matrix.size().height; row++) {
		for (int col = 0; col < matrix.size().width; col++) {
			if (matrix.at<float>(row, col) == 0) {
				return false;
			}
		}
	}
	
	return true;
}